﻿package no.doomsday.aronning.engines.verlets.constraints {
	import flash.display.Graphics;
	import no.doomsday.aronning.utilities.parameters.Parameter;
	import no.doomsday.aronning.engines.verlets.IRenderable;
	import no.doomsday.aronning.engines.verlets.IUpdatable;
	import no.doomsday.aronning.engines.verlets.utils.LinkedListNode;
	import no.doomsday.aronning.engines.verlets.Verlet;
	
	/**
	* ...
	* @author Andreas Rønning
	*/
	public class StickConstraint extends AbstractConstraint implements IUpdatable, IRenderable {
		
		public var restlength:Number = 30;
		protected var restlengthSquared:Number;
		public var fastMode:Boolean = true;
		public var stiffness:Parameter = new Parameter(1);
		private var _a:Verlet;
		private var _b:Verlet;
		public function StickConstraint(a:Verlet, b:Verlet, stiffness:Parameter = null, autoRestLength:Boolean = true, restlength:Number = 100, fastMode:Boolean = true) {
			if (!(addVerlet(a) && addVerlet(b))) {
				throw new Error("Stick constraint created with invalid verlets");
			}
			this.fastMode = fastMode;
			if(stiffness){
				this.stiffness = stiffness;
			}
			if (autoRestLength) {
				var diffX:Number = b.x - a.x;
				var diffY:Number = b.y - a.y;
				var dist:Number = Math.sqrt(diffX * diffX + diffY * diffY);
				this.restlength = dist;
			}else {
				this.restlength = restlength;
			}
			restlengthSquared = this.restlength * this.restlength;
			_b = b;
			_a = a;
		}
		public function render(g:Graphics):void {
			g.lineStyle(0, 0xFF0000);
			g.moveTo(_a.x, _a.y);
			g.lineTo(_b.x, _b.y);
		}
		public function update(timeStep:Number = 0):void {
			var diffX:Number = _b.x - _a.x;
			var diffY:Number = _b.y - _a.y;
			if (fastMode) {
				var deltaLengthSquared:Number = restlengthSquared / (diffX * diffX + diffY * diffY + restlengthSquared) - 0.5;
				diffX *= deltaLengthSquared;
				diffY *= deltaLengthSquared;
				_a.x -= diffX * stiffness.value;
				_a.y -= diffY * stiffness.value;
				_b.x += diffX * stiffness.value;
				_b.y += diffY * stiffness.value;
			}else {
				var deltalength:Number = Math.sqrt(diffX * diffX + diffY * diffY);
				var diff:Number = (deltalength - restlength) / deltalength;
				_b.x -= (diffX * 0.5 * diff) * stiffness.value;
				_b.y -= (diffY * 0.5 * diff) * stiffness.value;
				_a.x += (diffX * 0.5 * diff) * stiffness.value;
				_a.y += (diffY * 0.5 * diff) * stiffness.value;
			}	
		}
		public function setRestlengthFromTargets():Number {
			var diffX:Number = b.x - a.x;
			var diffY:Number = b.y - a.y;
			restlength = Math.sqrt(diffX * diffX + diffY * diffY);
			restlengthSquared = restlength * restlength;
			return restlength;
		}
		
		public function get b():Verlet { return _b; }
		
		public function set b(value:Verlet):void 
		{
			constrainedVerlets[1] = value;
			_b = constrainedVerlets[1];
		}
		
		public function get a():Verlet { return _a; }
		
		public function set a(value:Verlet):void 
		{
			constrainedVerlets[0] = value;
			_a = constrainedVerlets[0];
		}
	}	
}